String ip ="ws://omap:9090";
Boolean isRunning = false;

void setup(){
	size(500, 500);
	background(125);
	fill(255);
	PFont fontA = loadFont("courier");
	textFont(fontA, 20);
	text("Initializing environment...", 20, 20);
	noLoop();
	
	connect(ip);
	isRunning = false;
}

void run() {
	//ins.run();                // call run void in processing code
	subscribe('/sensorPacket',sensorPacketHandler);
}

void stop() {
	//ins.stop();               // call stop void in processing code
	move_robot(0,0);
}


void up(){
	move_robot(100,100);
}

void down(){
	move_robot(-100, -100);
}

void left(){
	move_robot(-1, 8);
}

void right(){
	move_robot(1, 8);
}

void move_robot(x,z) {
	publish('/cmd_vel','geometry_msgs/Twist','{"linear":{"x":' + x + ',"y":0,"z":0}, "angular":{"x":0,"y":0,"z":' + z + '}}');
}

void sensorPacketHandler(sensorPacket){
	//Bumpers
	if(sensorPacket.bumpRight && sensorPacket.bumpLeft){
		document.getElementById('bDer').style.backgroundColor='green';
		document.getElementById('bCent').style.backgroundColor='red';
		document.getElementById('bIzq').style.backgroundColor='green';
	}else if(sensorPacket.bumpRight){
		document.getElementById('bDer').style.backgroundColor='red';
		document.getElementById('bCent').style.backgroundColor='green';
		document.getElementById('bIzq').style.backgroundColor='green';
	}else if(sensorPacket.bumpLeft){
		document.getElementById('bDer').style.backgroundColor='green';
		document.getElementById('bCent').style.backgroundColor='green';
		document.getElementById('bIzq').style.backgroundColor='red';
	}else{
		document.getElementById('bDer').style.backgroundColor='green';
		document.getElementById('bCent').style.backgroundColor='green';
		document.getElementById('bIzq').style.backgroundColor='green';
	}
	
	//Charger
	
}